#include "CLineSegment3d.h"
#include "CMatrix3d.h"
#include "CVector3d.h"
#include "CMatrix4dh.h"
#include "CMatrix4d.h"




CVector3d::CVector3d( fixed ijDegrees, fixed ikDegrees )
{ 
	ASSERT( FALSE ); // Not yet implemented
}

 
CVector3d::CVector3d( const fixed* ijk )
{ 
	ASSERT( ijk );

	m_i = *ijk;
	m_j = *( ijk + 1 );
	m_k = *( ijk + 2 );
}


CVector3d& CVector3d::Cross( const CVector3d& v )
{
	int32 jk, kj, ki, ik, ij, ji;

	jk = CMathFixed::Mul( m_j, v.m_k );
	kj = CMathFixed::Mul( m_k, v.m_j );
	ki = CMathFixed::Mul( m_k, v.m_i );
	ik = CMathFixed::Mul( m_i, v.m_k );
	ij = CMathFixed::Mul( m_i, v.m_j );
	ji = CMathFixed::Mul( m_j, v.m_i );

	m_i = jk - kj;
	m_j = ki - ik;
	m_k = ij - ji;

	return *this;
}


CVector3d& CVector3d::ProjOnto( const CVector3d& v )
{
	ASSERT( FALSE ); // Not yet implemented

	return *this;
}


CVector3d& CVector3d::LerpTo( const CVector3d& v, fixed u )
{
	m_i = m_i + CMathFixed::Mul( u, v.m_i - m_i );
	m_j = m_j + CMathFixed::Mul( u, v.m_j - m_j );
	m_k = m_k + CMathFixed::Mul( u, v.m_k - m_k );

	return *this;
}


CVector3d&	CVector3d::RotateX( fixed degrees )
{
	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );
	fixed j = m_j;

	m_j = CMathFixed::Mul( j, cos ) + CMathFixed::Mul( m_k, sin );
	m_k = CMathFixed::Mul( j, -sin ) + CMathFixed::Mul( m_k, cos );

	return *this;
}


CVector3d&	CVector3d::RotateY( fixed degrees )
{
	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );
	fixed i = m_i;

	m_i = CMathFixed::Mul( i, cos ) + CMathFixed::Mul( m_k, -sin );
	m_k = CMathFixed::Mul( i, sin ) + CMathFixed::Mul( m_k, cos );

	return *this;
}


CVector3d&	CVector3d::RotateZ( fixed degrees )
{
	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );
	fixed i = m_i;

	m_i = CMathFixed::Mul( i, cos ) + CMathFixed::Mul( m_j, sin );
	m_j = CMathFixed::Mul( i, -sin ) + CMathFixed::Mul( m_j, cos );

	return *this;
}


fixed CVector3d::Angle( CVector3d& v ) const
{
	fixed mLen = Length() * v.Length();

	if( mLen )
		return CMathFixed::ACos( ( *this * v ) / mLen );
	else
		return 0;
}


fixed CVector3d::AngleTo( CVector3d& v ) const
{
	ASSERT( FALSE ); // Not yet implemented

	return 0;
}


fixed CVector3d::ShortestDistanceToLineSegment( class CLineSegment3d& segment, 
											    CVector3d& pntOfIntersection ) const
{
	return ShortestDistanceToLineSegment( segment.m_v0, segment.m_v1, pntOfIntersection );
}


fixed CVector3d::ShortestDistanceToLineSegment( const CVector3d& segV0, const CVector3d& segV1,
												CVector3d& pntOfIntersection ) const
{
	fixed dwv, dvv;
	CVector3d v = segV1 - segV0; 
	CVector3d w = *this - segV0; 

	dwv = w * v;
	dvv = v * v;

	if( dwv <= 0 )
	{
		w = segV0 - *this;
		pntOfIntersection = segV0;
	}
	else
	if( dvv <= dwv )
	{
		w = segV1 - *this;
		pntOfIntersection = segV1;
	}
	else
	{
		w = *this;

		pntOfIntersection = v;
		pntOfIntersection *= CMathFixed::Div( dwv, dvv );
		pntOfIntersection += segV0;

		w -= pntOfIntersection;
	}

	return w.Length();
}


fixed CVector3d::IsOnLineSegment( class CLineSegment3d& segment ) const
{
    return IsOnLineSegment( segment.m_v0, segment.m_v1 );
}



fixed CVector3d::IsOnLineSegment( const CVector3d& segV0, const CVector3d& segV1 ) const
{
	ASSERT( FALSE ); // Not yet implemented

    return 0;
}

fixed CVector3d::Max() const {
	fixed max = m_i;
	if ( m_j > max ) max = m_j;
	if ( m_k > max ) max = m_k;
	return max;
}

CVector3d& CVector3d::operator*=( const class CMatrix3d& a )
{
	fixed i = m_i;
	fixed j = m_j;

	m_i = CMathFixed::Mul( a[0], i ) + CMathFixed::Mul( a[3], j ) + CMathFixed::Mul( a[6], m_k );
	m_j = CMathFixed::Mul( a[1], i ) + CMathFixed::Mul( a[4], j ) + CMathFixed::Mul( a[7], m_k );
	m_k = CMathFixed::Mul( a[2], i ) + CMathFixed::Mul( a[5], j ) + CMathFixed::Mul( a[8], m_k );

	return *this;
}


CVector3d& CVector3d::operator*=( const class CMatrix4dh& a )
{
	fixed i = m_i;
	fixed j = m_j;

	m_i = CMathFixed::Mul( a[0], i ) + CMathFixed::Mul( a[4], j ) + CMathFixed::Mul( a[8], m_k ) + a[12];
	m_j = CMathFixed::Mul( a[1], i ) + CMathFixed::Mul( a[5], j ) + CMathFixed::Mul( a[9], m_k ) + a[13];
	m_k = CMathFixed::Mul( a[2], i ) + CMathFixed::Mul( a[6], j ) + CMathFixed::Mul( a[10], m_k ) + a[14];

	return *this;
}


CVector3d& CVector3d::operator*=( const class CMatrix4d& a )
{
	return operator*=( CMatrix4d_ToCMatrix4dh( a ) );
}


CVector3d operator*( const class CMatrix3d& a, const CVector3d& x )
{
	return CVector3d( CMathFixed::Mul( a[0], x.m_i ) + CMathFixed::Mul( a[3], x.m_j ) + CMathFixed::Mul( a[6], x.m_k ),
					  CMathFixed::Mul( a[1], x.m_i ) + CMathFixed::Mul( a[4], x.m_j ) + CMathFixed::Mul( a[7], x.m_k ),
					  CMathFixed::Mul( a[2], x.m_i ) + CMathFixed::Mul( a[5], x.m_j ) + CMathFixed::Mul( a[8], x.m_k ) );
}


CVector3d operator*( const class CMatrix4dh& a, const CVector3d& x )
{
	return CVector3d( CMathFixed::Mul( a[0], x.m_i ) + CMathFixed::Mul( a[4], x.m_j ) + CMathFixed::Mul( a[8], x.m_k ) + a[12],
					  CMathFixed::Mul( a[1], x.m_i ) + CMathFixed::Mul( a[5], x.m_j ) + CMathFixed::Mul( a[9], x.m_k ) + a[13],
					  CMathFixed::Mul( a[2], x.m_i ) + CMathFixed::Mul( a[6], x.m_j ) + CMathFixed::Mul( a[10], x.m_k ) + a[14] );
}
